#ifndef BALANCE_H_
#define BALANCE_H_

#include "Motor.h"

typedef struct{
	
	float vx_actual;
	
  float vz_actual;

	float vx_target;
	
	float vz_target;
	
	float vx_error;
	
	float vz_error;
	
	float vx_err_sum;
	
	float vz_err_sum;

	float err_last; 
	
	float vx_kp;

	float vx_ki;

	float vx_kd;

	float vx_p_out;

	float vx_i_out;

	float vx_d_out;
	
	float vz_kp;

	float vz_ki;

	float vz_kd;

	float vz_p_out;

	float vz_i_out;

	float vz_d_out;

	float vx_output;

	float vx_maxOutput;
	
	float vz_output;

	float vz_maxOutput;
	
} Chassis_Ctrl_t;

extern Chassis_Ctrl_t Chassis_Ctrl;
	

void Drive_Motor_IK(float Vx,float Vy,float Vz);
float target_limit_float(float insert,float low,float high);
//void PS2_Control(void);
void Drive_Motor_FK(void);
void ChassisPidCtrl(Chassis_Ctrl_t *Chassis_Ctrler);
float MaxMinLimit_chassis(float val, float limit);

#endif

